package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.buttons.Button;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.Gyro;

public class Input
{
    //Joysticks.
    private Joystick leftJoystick;
    private Joystick rightJoystick;
    private Joystick coDriverJoystick;
    public KinectStick leftArm;
    public KinectStick rightArm;
    private Joystick leftOperJoystick;
    private Joystick rightOperJoystick;

    //private JoystickButton trigger;
    //private Command triggerPressed;

    //Analog and Digital modules on the robot.
    private AnalogModule anaMod;
    private DigitalModule digiMod;

    private DigitalInput ballPosition1;
    private DigitalInput ballPosition2;
    private DigitalInput ballPosition3;
    //private DigitalInput rampArmPos1;
    //private DigitalInput rampArmPos2;

    private Encoder shooterEncoder;
    private DigitalInput shooterEncInputA;
    private DigitalInput shooterEncInputB;

    private Gyro gyro;

    //Constants for getAxis.
    static final public int xAxis = 0;
    static final public int yAxis = 1;
    static final public int zAxis = 2;
    static final public int rightStick = 3;
    static final public int leftStick = 4;
    static final public int coDriverStick = 5;

    static final public int startEnc = 0;
    static final public int stopEnc = 1;
    static final public int freeEnc = 2;

    private boolean gyroAttached = true;

    public Input() {
        //Initialize the joysticks and modules with their respective ports.
        this.leftJoystick = new Joystick(1);
        this.rightJoystick = new Joystick(2);
        this.coDriverJoystick = new Joystick(3);
        this.anaMod = AnalogModule.getInstance(1);
        this.digiMod = DigitalModule.getInstance(1);
        this.ballPosition1 = new DigitalInput(1);
        this.ballPosition2 = new DigitalInput(2);
        this.ballPosition3 = new DigitalInput(3);
        //this.rampArmPos1 = new DigitalInput(4);
        //this.rampArmPos2 = new DigitalInput(5);
        this.shooterEncInputA = new DigitalInput(4);
        this.shooterEncInputB = new DigitalInput(5);
        this.shooterEncoder = new Encoder(shooterEncInputA,shooterEncInputB);
        this.shooterEncoder.setDistancePerPulse((3.1415926 * 6) / 1440);    // 1440 Pulses Per Revolution  --  6in Diameter of wheel
        try {
            gyro = new Gyro(2);
        }
        catch (Exception e) {
            System.out.println(e);
            gyroAttached = false;
        }
    }

    public int getRange() {
        //Return the value of the range sensor.
        return anaMod.getAverageValue(1);
    }


    public double getAxis(int joystick, int axis) {
        //Returns the value of the given joystick's given axis.
        Joystick curJoystick;
        switch(joystick)
        {
            case rightStick:
                curJoystick = this.rightJoystick;
                break;
            case leftStick:
                curJoystick = this.leftJoystick;
                break;
            case coDriverStick:
                curJoystick = this.coDriverJoystick;
                break;
            default:
                System.out.println("Unexpected joystick value");
                return -1;

        }

        switch(axis)
        {
            case xAxis:
                return curJoystick.getX() *-1;
            case yAxis:
                return curJoystick.getY() *-1;
            case zAxis:
                return curJoystick.getZ() *-1;
            default:
                System.out.println("Unexpected joystick axis value");
                return -1;
        }
    }
    public int getKinectState() {
        if (leftArm.getY() > 0.1 && rightArm.getY() > 0.1) {
            //If both arms are up
            return 1;
        } else if (leftArm.getY() < -0.1 && rightArm.getY() < -0.1) {
            //If both arms are down
            return 2;
        } else if (leftArm.getY() > 0.1 && rightArm.getY() < -0.1) {
            //Left arm up, right arm down
            return 3;
        } else if (leftArm.getY() < -0.1 && rightArm.getY() > 0.1) {
            //Left arm down, right arm up
            return 4;
        } else {
            return 0;
        }
    }
    public double getThrottle(int stick){
        //Returns the value of the given joystick's throttle
        Joystick currentStick;
        switch(stick)
        {
            case rightStick:
                currentStick = this.rightJoystick;
                break;
            case leftStick:
                currentStick = this.leftJoystick;
            case coDriverStick:
                currentStick = this.coDriverJoystick;
                break;

            default:
                System.out.println("Unexpected joystick value");
                return 0.0;
        }
        return currentStick.getThrottle();
   }

    public boolean getStickButtonState(int stick, int button) {
        //Returns the value of the given joystick's given axis.
        Joystick currentStick;
        switch(stick)
        {
            case rightStick:
                currentStick = this.rightJoystick;
                break;
            case leftStick:
                currentStick = this.leftJoystick;
                break;
            case coDriverStick:
                currentStick = this.coDriverJoystick;
                break;

            default:
                System.out.println("Unexpected joystick value");
                return false;
        }
        return currentStick.getRawButton(button);
    }

    public boolean getStickTrigger(int stick) {
        Joystick currentStick;
        switch(stick)
        {
            case rightStick:
                currentStick = this.rightJoystick;
                break;
            case leftStick:
                currentStick = this.leftJoystick;
                break;
            default:
                System.out.println("Unexpected joystick value");
                return false;
        }

        //Command c = JoystickButton(currentStick,1);
        return currentStick.getTrigger();

    }
    public int getLeftDistance(){
        return this.anaMod.getAverageValue(2);
    }
    public int getRightDistamce(){
        return this.anaMod.getAverageValue(3);
    }
    //Commented this out because a less complex method was written.
    /*public boolean[] getBallPosition() {
        boolean[] returns = {};
        returns[0] = ballPosition1.get();
        returns[1] = ballPosition2.get();
        returns[2] = ballPosition3.get();
        return returns;
    }
    *
    */
    public boolean getInput(int id)
    {
       boolean bSensorState = false;
       switch(id)
       {
           case 1:
               bSensorState = ballPosition1.get();
               break;
           case 2:
               bSensorState = ballPosition2.get();
               break;
           case 3:
               bSensorState = ballPosition3.get();
               break;
           default:
               break;
       }
       return bSensorState;
    }
    public boolean getRampArmState() {
        /*if (rampArmPos1.get() && !rampArmPos2.get()) {
            return true;
        } else if(!rampArmPos1.get() && rampArmPos2.get()) {
            return false;
        } else {
            System.out.println("Ramp Arm Error - Unknown State!");
            return false;
        }*/
        return false;
    }

    public int getEncoderPulseCount() {
        int retVal = shooterEncoder.get();
        shooterEncoder.reset();
        return retVal;

    }

    public double getEncoderDistance() {
        return shooterEncoder.getDistance();
    }

    public double getEncoderRate() {
        return shooterEncoder.getRate();
    }

    public void setEncoder(int func) {
        switch(func) {
            case 0:  //start
                shooterEncoder.start();
                System.out.println("Encoder Started");
                break;
            case 1:  //stop
                shooterEncoder.stop();
                System.out.println("Encoder Stopped");
                break;
            case 2:  //free
                shooterEncoder.free();
                System.out.println("Encoder Memory Freed");
                break;
            default: //error
                System.out.println("Error in setEncoder -- input.java:248");
                break;
        }
    }


	/**
	 * Get the voltage from the potentiometer
	 * @return Voltage from analog module
	 */
	public double getAngleVoltage() {
		return this.anaMod.getAverageVoltage(1);
	}

    public double getAngle() {
        if(this.gyroAttached) {
            return this.gyro.getAngle();
        } else {
            return -1.01101;
        }
    }

    public void resetGyro() {
        if(this.gyroAttached) {
            this.gyro.reset();
        }
    }

}
